﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;
using System.Text;

namespace SharpSoft.Maths
{
    /// <summary>
    /// 卡尔曼滤波算法（二维）
    /// </summary>
    public class KalmanFilter2
    {
        Kalman2D xk = new Kalman2D();
        Kalman2D yk = new Kalman2D();

        public KalmanFilter2()
        {

        }
        /// <summary>
        /// 初始位置
        /// </summary>
        public Vector2 InitPosition { get; set; } = new Vector2(0, 0);
        /// <summary>
        /// 初始速度
        /// </summary>
        public Vector2 InitVelocity { get; set; } = new Vector2(0f, 0f);
        /// <summary>
        /// 测算协方差
        /// </summary>
        public float MeasurementCovariance { get; set; } = 5f;
        /// <summary>
        /// 初始方差
        /// </summary>
        public float InitialVariance { get; set; } = 10f;
        /// <summary>
        /// 初始位置
        /// </summary>
        public Vector2 InitialPosition { get; set; }
        /// <summary>
        /// 重置
        /// </summary>
        /// <param name="initPoint"></param>
        public void Reset()
        {
            xk.Reset(InitPosition.X//X位置最小值
                , InitVelocity.X//速度最小值
                , MeasurementCovariance
                , InitialVariance
                , InitialPosition.X//初始位置
                );
            yk.Reset(InitPosition.Y//Y位置最小值
                , InitVelocity.Y//速度最小值
                , MeasurementCovariance
                , InitialVariance
                , InitialPosition.Y//初始位置
                );
        }
        Vector2? lastPoint = null;
        double lastTime = 0;
        /// <summary>
        /// 更新坐标并输出当前滤波值
        /// </summary>
        /// <param name="value"></param>
        /// <param name="time"></param>
        /// <returns></returns>
        public Vector2 Update(Vector2 value, double time)
        {
            double vx = 0d, vy = 0d;
            double timespan = time - lastTime;
            if (lastPoint.HasValue)
            {//计算速度
                vx = (value.X - lastPoint.Value.X) / timespan;
                vy = (value.Y - lastPoint.Value.Y) / timespan;
            }
            lastPoint = value;
            lastTime = time;
            var kx = xk.Update(value.X, vx, 1);
            var ky = yk.Update(value.Y, vy, 1);
            return new Vector2(Convert.ToSingle(kx), Convert.ToSingle(ky));
        }

    }
}
